"""
名称：CobotGPT_实时停止测试(虚拟机)
功能：搭载碰撞检测模型，并实现收到碰撞信号后实时停止，虚拟机仿真，摄像头不参与
缺陷：
"""

# import csv
import timeit
from multiprocessing import Process, Queue

from robotcontrol import *


# from kinectcontrol import *
# import random


def delay(microseconds):
    """
    功能：延迟xx微秒，1s = 1*10^3 ms = 1*10^6 us = 1*10^9 ns
    输入：
    输出
    """
    start = timeit.default_timer()
    while True:
        elapsed = (timeit.default_timer() - start) * 10 ** 6
        if elapsed >= microseconds:
            break
    return 0


class CobotGPT:

    def __init__(self):
        """
        功能：连接机械臂
        输入：
        输出：连接状态码
        """

        # 链接服务器
        # 注意配置网线口的适配器属性：关掉IPV6，手动设置IPV4地址和机械臂在同一个网段
        # self.ip = '192.168.213.128'
        self.ip = '192.168.199.128'  # 三楼虚拟机ip地址
        self.port = 8899    # 默认端口
        self.joint_radian_list = [(80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                                  (
                                      160 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180,
                                      0 * pi / 180),
                                  (160 * pi / 180, -30 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180,
                                   0 * pi / 180),
                                  (160 * pi / 180, -30 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180,
                                   0 * pi / 180),
                                  (160 * pi / 180, -30 * pi / 180, 80 * pi / 180, 20 * pi / 180, 0 * pi / 180,
                                   0 * pi / 180),
                                  (160 * pi / 180, -30 * pi / 180, 80 * pi / 180, 20 * pi / 180, 90 * pi / 180,
                                   0 * pi / 180),  # 第一阶段
                                  (160 * pi / 180, 0 * pi / 180, 80 * pi / 180, 20 * pi / 180, 90 * pi / 180,
                                   0 * pi / 180),
                                  (90 * pi / 180, 0 * pi / 180, 80 * pi / 180, 20 * pi / 180, 90 * pi / 180,
                                   0 * pi / 180),
                                  (90 * pi / 180, -20 * pi / 180, 80 * pi / 180, 10 * pi / 180, 90 * pi / 180,
                                   0 * pi / 180)]   # length: 9

    def robot_set_position(self, cnt, queue1: Queue, queue2: Queue):
        """
        功能：设置机械臂的固定轨迹点，并实时获取关节角信息
        输入：cnt测试次数，queue1是否碰撞，queue2机械臂关节角
        输出：
        """

        # 初始化logger
        logger_init()
        # 启动测试
        logger.info("{0} robot_set_position() running...".format(Auboi5Robot.get_local_time()))
        # 系统初始化
        Auboi5Robot.initialize()
        # 创建机械臂控制类
        robot = Auboi5Robot()
        # 创建上下文
        handle = robot.create_context()
        # 打印上下文
        logger.info("robot.rshd={0}".format(handle))

        try:
            # 连接服务器
            ip = self.ip
            port = self.port
            result = robot.connect(ip, port)
            if result != RobotErrorType.RobotError_SUCC:
                logger.info("connect server{0}:{1} failed.".format(ip, port))
            else:
                # 上电
                robot.robot_startup()
                # 设置碰撞等级
                robot.set_collision_class(10)
                # 初始化全局配置文件
                robot.init_profile()
                # 循环测试
                while cnt > 0:
                    cnt -= 1
                    # 设置关节最大加速度
                    robot.set_joint_maxacc((1.5, 1.5, 1.5, 1.5, 1.5, 1.5))
                    # 设置关节最大速度
                    robot.set_joint_maxvelc((0.5, 0.5, 0.5, 0.5, 0.5, 0.5))
                    # 回到初始位姿
                    joint_radian_ori = (0, 0, 0, 0, 0, 0)  # 初始位姿,turple类型
                    # print(type(joint_radian_ori))
                    logger.info("move joint to {0}".format(joint_radian_ori))
                    robot.move_joint(joint_radian_ori)

                step = 0
                while True:
                    joint_radian = self.joint_radian_list[step]
                    logger.info("[step{1}]: move joint to {0}".format(self.joint_radian_list[step], step))
                    robot.move_joint(joint_radian)
                    step += 1
                    # if step == len(self.joint_radian_list):
                    if step == 6:
                        step = 0

        # 出现错误的处理
        except RobotError as e:
            logger.error("{0} robot Event:{1} in robot_set_position()".format(robot.get_local_time(), e))

        # 无论是否有异常，都会执行的代码
        finally:
            # 断开服务器链接
            # if robot.connected:
            #     # 关闭机械臂
            #     robot.robot_shutdown()
            #     # 断开机械臂链接
            #     robot.disconnect()
            # # 释放库资源
            # Auboi5Robot.uninitialize()
            logger.info("{0} test completed.".format(Auboi5Robot.get_local_time()))

    def collision_detection(self, queue1: Queue, queue2: Queue):

        collision_flag = 0  # 默认未发生碰撞
        """
                功能：设置机械臂的固定轨迹点，并实时获取关节角信息
                输入：cnt测试次数，queue1是否碰撞，queue2机械臂关节角
                输出：
                """

        # 初始化logger
        logger_init()
        # 启动测试
        logger.info("{0} collision_detection() running...".format(Auboi5Robot.get_local_time()))

        # 创建机械臂控制类
        robot = Auboi5Robot()
        # 创建上下文
        handle = robot.create_context()
        # 打印上下文
        logger.info("robot.rshd={0}".format(handle))

        try:
            # 连接服务器
            ip = self.ip
            port = self.port
            result = robot.connect(ip, port)
            if result != RobotErrorType.RobotError_SUCC:
                logger.info("connect server{0}:{1} failed.".format(ip, port))
            else:
                pass
                step = 0
                step_debug = 0
                # 随机碰撞 = random.randint(1, 8)
                # 随机碰撞 = random.randint(1, 8)
                # while step < int(len(self.joint_radian_list)):
                #     time.sleep(3)
                #
                #     logger.info("即将发生碰撞，机械臂暂停1s")
                #     robot.move_pause()
                #     time.sleep(1)
                #     logger.info("机械臂恢复")
                #     robot.move_continue()
        # 出现错误的处理
        except RobotError as e:
            logger.error("{0} robot Event:{1} in collision_detection()".format(robot.get_local_time(), e))

        # 无论是否有异常，都会执行的代码
        finally:
            # 断开服务器链接
            # if robot.connected:
            #     # 关闭机械臂
            #     robot.robot_shutdown()
            #     # 断开机械臂链接
            #     robot.disconnect()
            # # 释放库资源
            # Auboi5Robot.uninitialize()
            # logger.info("{0} test completed.".format(Auboi5Robot.get_local_time()))
            pass


if __name__ == '__main__':
    # game = BodyGameRuntime()
    cobot = CobotGPT()
    WAPOINT = GetRobotWaypointProcess()

    # 创建通信队列
    q1 = Queue()
    q2 = Queue()
    q3 = Queue()
    # # 创建子进程
    p1 = Process(target = cobot.robot_set_position, kwargs = {"cnt": 10, "queue1": q1, "queue2": q2})
    # p2 = Process(target=cobot.robot_get_waypoint)
    p3 = Process(target = cobot.collision_detection, kwargs = {"queue1": q1, "queue2": q2})
    # p4 = Process(target=cobot.dataset, kwargs={"queue": q3})

    p1.start()
    # p2.start()
    p3.start()
    # p4.start()
    # 主进程
    # game.run(q3)
    # p1.join()
    # p2.join()
    WAPOINT.run(ip = '192.168.199.128', port = 8899)
